#include "iMath.h"

/**
  * @brief  将坐标按首要升序排列
  * @param  需要排列的数组的首地址
  * @note   冒泡算法的复杂度为O（n^2）
  * @retval 无
  */
void bubble_sort(int *array, size_t num)
{
    int i = 0;
    int j = 0;
    int temp;
    for (; j < num; ++j)
    {
        for (i = num-1; i > j; --i)
        {
            if (array[i] < array[i - 1])
            {
                iSWAP(temp, array[i], array[i - 1]);
            }
        }
    }
}

/**
  * @brief  将坐标按首要升序排列
  * @param  x：首要
  * @param  y：伴随
  * @retval 无
  */
void xybubble_sort(int *x,int *y, size_t num)
{
    int i = 0;
    int j = 0;
    int temp;
    for (; j < num; ++j)
    {
        for (i = num-1; i > j; --i)
        {
            if (x[i] < x[i - 1])
            {
                iSWAP(temp, x[i], x[i - 1]);
                iSWAP(temp, y[i], y[i - 1]);
            }
        }
    }
}
void xybubble_sort_u16(uint16_t *x,uint16_t *y, size_t num)
{
    int i = 0;
    int j = 0;
    uint16_t temp;
    for (; j < num; ++j)
    {
        for (i = num-1; i > j; --i)
        {
            if (x[i] < x[i - 1])
            {
                iSWAP(temp, x[i], x[i - 1]);
                iSWAP(temp, y[i], y[i - 1]);
            }
        }
    }
}
//插入排序
/*
    该算法的最坏情况，如逆序，那么复杂度为O(N^2)
    最好的情况，如已经预先排好序或者基本排好，那么复杂度为O（N）
*/
void insertion_sort(int *array, int num)
{
    int i, j;
    int temp;
    i = 0;
    j = 0;
    for (; i < num; i++)
    {
        for (j = i; (j > 0) && (array[j] < array[j - 1]); j--)
        {
            temp = array[j - 1];
            array[j - 1] = array[j];
            array[j] = temp;
        }
    }
}

//使用希尔增量时希尔排序的最坏时间复杂度为O（N^2）
void shell_sort(int *array, int num)
{
    int increment = 0;
    int temp = 0;
    int j = 0;
    int k = 0;
    int m = 0;
    for (increment = num / 2; increment > 0; increment /= 2)
    {
        printf("increment:%d \n ", increment);
        for (j = 0; j < increment; j++)
        {
            for (k = j; k < num; k = k + increment)
            {
                printf("k:%d \n ", k);
                for (m = k; (m > j) && (array[m] < array[m - increment]); m = m - increment)
                {
                    temp = array[m];
                    array[m] = array[m - increment];
                    array[m - increment] = temp;
                }
            }
        }
    }
}

int compare_int(const void *a, const void *b)
{
    return *(int *)a < *(int *)b ? -1 : *(int *)a > *(int *)b;
}
void qsort_int(void *base, size_t Long)
{
    qsort((int *)base, Long, sizeof(int), compare_int);
}
int compare_uint8(const void *a, const void *b)
{
    return (*(unsigned char *)a - *(unsigned char *)b);
}
void qsort_uint8(void *base, size_t Long)
{
    qsort((unsigned char *)base, Long, sizeof(char), compare_uint8);
}
int compare_uint16(const void *a, const void *b)
{
    return (*(uint16_t *)a - *(uint16_t *)b);
}
void qsort_uint16(void *base, size_t Long)
{
	qsort((uint16_t *)base, Long, sizeof(uint16_t), compare_uint16);
}
int compare_int16(const void *a, const void *b)
{
    return (*(int16_t *)a - *(int16_t *)b);
}
void qsort_int16(void *base, size_t Long)
{
	qsort((int16_t *)base, Long, sizeof(int16_t), compare_int16);
}
int compare_uint32(const void *a, const void *b)
{
    return (*(unsigned int *)a - *(unsigned int *)b);
}
void qsort_uint32(void *base, size_t Long)
{
    qsort((unsigned int *)base, Long, sizeof(unsigned int), compare_uint32);
}

size_t FindMaxIndex(int *data, size_t length)
{
    int temp = data[0];
    for (int i = 1; i < length; i++)
    {
        data[i] > data[temp] ? temp = i : temp;
    }
    return temp;
}

size_t FindMinIndex(int *data, size_t length)
{
    int temp = data[0];
    for (int i = 1; i < length; i++)
    {
        data[i] < data[temp] ? temp = i : temp;
    }
    return temp;
}

/*http://codepad.org/pQsNnpqd
 快速寻找中位数算法
*/
#define ELEM_SWAP(a, b)             \
    {                               \
        register elem_type t = (a); \
        (a) = (b);                  \
        (b) = t;                    \
    }
typedef int elem_type;
elem_type FindMedian(elem_type arr[], int n)
{
    int low, high;
    int median;
    int middle, ll, hh;

    low = 0;
    high = n - 1;
    median = (low + high) / 2;
    for (;;)
    {
        if (high <= low) /* One element only */
            return arr[median];

        if (high == low + 1)
        { /* Two elements only */
            if (arr[low] > arr[high])
                ELEM_SWAP(arr[low], arr[high]);
            return arr[median];
        }

        /* Find median of low, middle and high items; swap into position low */
        middle = (low + high) / 2;
        if (arr[middle] > arr[high])
            ELEM_SWAP(arr[middle], arr[high]);
        if (arr[low] > arr[high])
            ELEM_SWAP(arr[low], arr[high]);
        if (arr[middle] > arr[low])
            ELEM_SWAP(arr[middle], arr[low]);

        /* Swap low item (now in position middle) into position (low+1) */
        ELEM_SWAP(arr[middle], arr[low + 1]);

        /* Nibble from each end towards middle, swapping items when stuck */
        ll = low + 1;
        hh = high;
        for (;;)
        {
            do
                ll++;
            while (arr[low] > arr[ll]);
            do
                hh--;
            while (arr[hh] > arr[low]);

            if (hh < ll)
                break;

            ELEM_SWAP(arr[ll], arr[hh]);
        }

        /* Swap middle item (in position low) back into correct position */
        ELEM_SWAP(arr[low], arr[hh]);

        /* Re-set active partition */
        if (hh <= median)
            low = ll;
        if (hh >= median)
            high = hh - 1;
    }
}

#undef ELEM_SWAP


double line_xyxy_GetX(double x0, double y0, double x1, double y1, double y)
{

	if(x0 == x1)//垂直x
	{
		return x0;
	} 
	else if(y0 == y1)//垂直y
	{
		return 0xFFFF;//错误
	}
	else
	{
		return (y-y0)*(x1-x0)/(y1-y0) + x0;
	}
	
}

double line_xyxy_GetY(double x0, double y0, double x1, double y1, double x)
{
	if(x0 == x1)//垂直x
	{
		return 0xFFFF;//错误
	} 
	else if(y0 == y1)//垂直y
	{
		return y0;
	}
	else
	{
		return (x-x0)*(y1-y0)/(x1-x0) + y0;
	}
}


long sum_int32(int* arr, size_t size)
{
	long temp = 0;
	for(int i = 0;i<size;i++)
	{
		temp += arr[i]; 
	}
	return temp;
}
long sum_int16(int16_t* arr, size_t size)
{
	long temp = 0;
	for(int i = 0;i<size;i++)
	{
		temp += arr[i]; 
	}
	return temp;
}

uint64_t sum_u16(uint16_t *arr, size_t size)
{
	uint64_t temp = 0;
	for(uint16_t i = 0;i<size;i++)
	{
		temp += arr[i]; 
	}
	return temp;
}
uint64_t sum_u8(uint8_t *arr, size_t size)
{
	uint64_t temp = 0;
	for(uint16_t i = 0;i<size;i++)
	{
		temp += arr[i]; 
	}
	return temp;
}
double filter_average_int32(int* arr, size_t size)
{
	qsort_int(arr,size);
	long temp = sum_int32(arr+1,size-2);
	return (double)temp/(size-2);
}

double filter_average_int16(int16_t* arr, size_t size)
{
	qsort_int16(arr,size);
	long temp = sum_int16(arr+1,size-2);
	return (double)temp/(size-2);
}

double filter_average_u16(uint16_t* arr, size_t size)
{
	qsort_uint16(arr,size);
	long temp = sum_u16(arr+1,size-2);
	return (double)temp/(size-2);
}
double filter_average_u8(uint8_t* arr, size_t size)
{
	qsort_uint8(arr,size);
	long temp = sum_u8(arr+1,size-2);
	return (double)temp/(size-2);
}

/**
 * @brief 1 Dimension Kalman filter
 * @param state Klaman filter structure
 * @param z_measure Measure value
 * @retval float Estimated result
 * @note  Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
 *        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好       
 */
float kalman1_filter(kalman1_t *state, float z_measure)
{
    /* Predict */
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Measurement */
    state->K = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->K * (z_measure - state->H * state->x);
    state->p = (1 - state->K * state->H) * state->p;

    return state->x;
}
